//----------------------------------------------------------------------------
//
// (C) Copyrights MARIN/MSCN  2004 
//
//----------------------------------------------------------------------------
//
// $Author: A. Abbing $
//
// $Description: $
//
// $End: $
//
//----------------------------------------------------------------------------

#ifndef __mst_Matrix4_h__
#define __mst_Matrix4_h__

#include "string.h"
#include "Math.h"
#include "Notifier.h"
#include <iostream>

namespace mst {

/*
 * | a00 a01 |
 * | a10 a11 |
*/
#define MST_DET2(a00, a01, a10, a11) (a00*a11 - a10*a01)

/*
 * | a00 a01 a02 |
 * | a10 a11 a12 |
 * | a20 a21 a22 |
*/
#define MST_DET3(a00, a01, a02, a10, a11, a12, a20, a21, a22) \
				(a00*MST_DET2(a11, a12, a21, a22) \
				- a01*MST_DET2(a10, a12, a20, a22) \
				+ a02*MST_DET2(a10, a11, a20, a21))
				
/*! 
\class Matrix4 mst/Matrix4.h
\brief A 4x4 homogeneous matrix template class
\ingroup math
*/

template<class Type> class Matrix4 {
public:
	/*! 
	\fn Matrix4()	
	\brief Constructor whichs clears the matrix by setting its memory to 0
	*/
	Matrix4() { memset((void*) m_m, 0, 16 * sizeof(Type)); }
	
	/*! 
	\fn Matrix4(Type a00, Type a01, Type a02, Type a03,
		Type a10, Type a11, Type a12, Type a13,
		Type a20, Type a21, Type a22, Type a23,
		Type a30, Type a31, Type a32, Type a33)
	\brief Constructor
	\param a00 the value for m[0][0]
	\param a01 the value for m[0][1]
	\param a02 the value for m[0][2]
	\param a03 the value for m[0][3]
	\param a10 the value for m[1][0]
	\param a11 the value for m[1][1]
	\param a12 the value for m[1][2]
	\param a13 the value for m[1][3]
	\param a20 the value for m[2][0]
	\param a21 the value for m[2][1]
	\param a22 the value for m[2][2]
	\param a23 the value for m[2][3]
	\param a30 the value for m[3][0]
	\param a31 the value for m[3][1]
	\param a32 the value for m[3][2]
	\param a33 the value for m[3][3]
	*/
	Matrix4(Type a00, Type a01, Type a02, Type a03,
		Type a10, Type a11, Type a12, Type a13,
		Type a20, Type a21, Type a22, Type a23,
		Type a30, Type a31, Type a32, Type a33) {
		m_m[0][0] = a00; m_m[0][1] = a01; m_m[0][2] = a02; m_m[0][3] = a03;
		m_m[1][0] = a10; m_m[1][1] = a11; m_m[1][2] = a12; m_m[1][3] = a13;
		m_m[2][0] = a20; m_m[2][1] = a21; m_m[2][2] = a22; m_m[2][3] = a23;
		m_m[3][0] = a30; m_m[3][1] = a31; m_m[3][2] = a32; m_m[3][3] = a33;
	}
	/*! 
	\fn Matrix4(const Type *m)	
	\brief Copy contructor by copying the memory contents from \f$\underline{m}\f$
	*/
	Matrix4(const Type *m) 
	{ 
		if (m) memcpy(m_m, m, sizeof(Type) * 16);
		else memset(m_m, 0, sizeof(Type) * 16);
	}

	virtual ~Matrix4() {}

	/*! 
	\fn void set(Type *m)
	\brief Copy the contents of \f$\underline{m}\f$ to \f$\underline{this}\f$
	*/
	void set(Type *m) { memcpy(m_m, m, sizeof(Type) * 16); }

	/*! 
	\fn void set(Type a00, Type a01, Type a02, Type a03,
		Type a10, Type a11, Type a12, Type a13,
		Type a20, Type a21, Type a22, Type a23,
		Type a30, Type a31, Type a32, Type a33)
	\brief Set the 4x4 matrix to the specified values
	\param a00 the value for m[0][0]
	\param a01 the value for m[0][1]
	\param a02 the value for m[0][2]
	\param a03 the value for m[0][3]
	\param a10 the value for m[1][0]
	\param a11 the value for m[1][1]
	\param a12 the value for m[1][2]
	\param a13 the value for m[1][3]
	\param a20 the value for m[2][0]
	\param a21 the value for m[2][1]
	\param a22 the value for m[2][2]
	\param a23 the value for m[2][3]
	\param a30 the value for m[3][0]
	\param a31 the value for m[3][1]
	\param a32 the value for m[3][2]
	\param a33 the value for m[3][3]
	*/
	void set(Type a00, Type a01, Type a02, Type a03,
		Type a10, Type a11, Type a12, Type a13,
		Type a20, Type a21, Type a22, Type a23,
		Type a30, Type a31, Type a32, Type a33) {
		m_m[0][0] = a00; m_m[0][1] = a01; m_m[0][2] = a02; m_m[0][3] = a03;
		m_m[1][0] = a10; m_m[1][1] = a11; m_m[1][2] = a12; m_m[1][3] = a13;
		m_m[2][0] = a20; m_m[2][1] = a21; m_m[2][2] = a22; m_m[2][3] = a23;
		m_m[3][0] = a30; m_m[3][1] = a31; m_m[3][2] = a32; m_m[3][3] = a33;
	}		
	/*! 
	\fn void setRow(int r, Type x, Type y, Type z)
	\brief Set row \a r of \f$\underline{this}\f$
	\param r The row index 0 <= \a r < 4
	\param x The value for m[r][0]
	\param y The value for m[r][1]
	\param z The value for m[r][2]

	The value for m[r][3] will be (Type) 0
	*/
	void setRow(int r, Type x, Type y, Type z) {
		m_m[r][0] = x; m_m[r][1] = y; m_m[r][2] = z; m_m[r][3] = (Type) 0;
	}
	/*! 
	\fn void setRow(int r, Type x, Type y, Type z, Type w)
	\brief Set row \a r of \f$\underline{this}\f$
	\param r The row index 0 <= \a r < 4
	\param x The value for m[r][0]
	\param y The value for m[r][1]
	\param z The value for m[r][2]
	\param w The value for m[r][3]
	*/
	void setRow(int r, Type x, Type y, Type z, Type w) {
		m_m[r][0] = x; m_m[r][1] = y; m_m[r][2] = z; m_m[r][3] = w;
	}
	/*! 
	\fn void getRow(int r, Type &x, Type &y, Type &z, Type &w) const
	\brief Get row \a r of \f$\underline{this}\f$
	\param r The row index 0 <= \a r < 4
	\param x The value of m[r][0]
	\param y The value of m[r][1]
	\param z The value of m[r][2]
	\param w The value of m[r][3]
	*/
	void getRow(int r, Type &x, Type &y, Type &z, Type &w) const {
		x = m_m[r][0]; y = m_m[r][1]; z = m_m[r][2]; w = m_m[r][3];
	}
	/*! 
	\fn void setCol(int c, Type x, Type y, Type z, Type w)
	\brief Set column \a c of \f$\underline{this}\f$
	\param c The column index 0 <= \a c < 4
	\param x The value for m[0][c]
	\param y The value for m[1][c]
	\param z The value for m[2][c]
	\param w The value for m[3][c]
	*/
	void setCol(int c, Type x, Type y, Type z, Type w) {
		m_m[0][c] = x; m_m[1][c] = y; m_m[2][c] = z; m_m[3][c] = w;
	}

	/*! 
	\fn void getCol(int c, Type &x, Type &y, Type &z, Type &w) const
	\brief Get column \a c of \f$\underline{this}\f$
	\param c The column index 0 <= \a c < 4
	\param x The value of m[0][c]
	\param y The value of m[1][c]
	\param z The value of m[2][c]
	\param w The value of m[3][c]
	*/
	void getCol(int c, Type &x, Type &y, Type &z, Type &w) const {
		x = m_m[0][c]; y = m_m[1][c]; z = m_m[2][c]; w = m_m[3][c];
	}

	/*! 
	\fn void copy(const Matrix4<Type>& m)
	\brief return \f$\underline{this} = \underline{m}\f$
	\param m The source matrix
	*/
	void copy(const Matrix4<Type>& m) { *this = m; }

	/*! 
	\fn bool equal(const Matrix4<Type>& m) const
	\brief return \f$\underline{this} == \underline{m}\f$
	*/
	bool equal(const Matrix4<Type>& m) const  {
		return ((m_m[0][0] == m[0][0]) &&
			(m_m[0][1] == m[0][1]) &&
			(m_m[0][2] == m[0][2]) &&
			(m_m[0][3] == m[0][3]) &&
			(m_m[1][0] == m[1][0]) &&
			(m_m[1][1] == m[1][1]) &&
			(m_m[1][2] == m[1][2]) &&
			(m_m[1][3] == m[1][3]) &&
			(m_m[2][0] == m[2][0]) &&
			(m_m[2][1] == m[2][1]) &&
			(m_m[2][2] == m[2][2]) &&
			(m_m[2][3] == m[2][3]) &&
			(m_m[3][0] == m[3][0]) &&
			(m_m[3][1] == m[3][1]) &&
			(m_m[3][2] == m[3][2]) &&
			(m_m[3][3] == m[3][3]));
	}
	/*! 
	\fn bool equal(const Matrix4<Type> &m, Type tol) const
	\brief return \f$\underline{this} ~== \underline{m}\f$ specifying the tolerance
	*/
	bool equal(const Matrix4<Type> &m, Type tol) const {
		return mst::equal(m_m[0][0],m[0][0],tol) && 
			 mst::equal(m_m[0][1], m[0][1], tol) && 
			 mst::equal(m_m[0][2], m[0][2], tol) && 
			 mst::equal(m_m[0][3], m[0][3], tol) && 
			 mst::equal(m_m[1][0], m[1][0], tol) &&
			 mst::equal(m_m[1][1], m[1][1], tol) &&
			 mst::equal(m_m[1][2], m[1][2], tol) && 
			 mst::equal(m_m[1][3], m[1][3], tol) &&
			 mst::equal(m_m[2][0], m[2][0], tol) &&
			 mst::equal(m_m[2][1], m[2][1], tol) &&
			 mst::equal(m_m[2][2], m[2][2], tol) &&
			 mst::equal(m_m[2][3], m[2][3], tol) &&
			 mst::equal(m_m[3][0], m[3][0], tol) &&
			 mst::equal(m_m[3][1], m[3][1], tol) &&
			 mst::equal(m_m[3][2], m[3][2], tol) &&
			 mst::equal(m_m[3][3], m[3][3], tol);
	}
	/*! 
	\fn bool almostEqual(const Matrix4<Type> &m, Type tol) const
	\brief return \f$\underline{this} ~== \underline{m}\f$ specifying the tolerance
	*/
	bool almostEqual(const Matrix4<Type> &m, Type tol) const 
		{ return equal(m, tol); }

	/*! 
	\fn bool equal(const Matrix4<Type> &m, const Matrix4<Type>& tol) const
	\brief return \f$\underline{this} ~== \underline{m}\f$ specifying the tolerance
	*/
	bool equal(const Matrix4<Type> &m, const Matrix4<Type>& tol) const {
		return mst::equal(m_m[0][0],m[0][0],tol[0][0]) && 
			 mst::equal(m_m[0][1], m[0][1], tol[0][1]) && 
			 mst::equal(m_m[0][2], m[0][2], tol[0][2]) && 
			 mst::equal(m_m[0][3], m[0][3], tol[0][3]) && 
			 mst::equal(m_m[1][0], m[1][0], tol[1][0]) &&
			 mst::equal(m_m[1][1], m[1][1], tol[1][1]) &&
			 mst::equal(m_m[1][2], m[1][2], tol[1][2]) && 
			 mst::equal(m_m[1][3], m[1][3], tol[1][3]) &&
			 mst::equal(m_m[2][0], m[2][0], tol[2][0]) &&
			 mst::equal(m_m[2][1], m[2][1], tol[2][1]) &&
			 mst::equal(m_m[2][2], m[2][2], tol[2][2]) &&
			 mst::equal(m_m[2][3], m[2][3], tol[2][3]) &&
			 mst::equal(m_m[3][0], m[3][0], tol[3][0]) &&
			 mst::equal(m_m[3][1], m[3][1], tol[3][1]) &&
			 mst::equal(m_m[3][2], m[3][2], tol[3][2]) &&
			 mst::equal(m_m[3][3], m[3][3], tol[3][3]);
	}
	/*! 
	\fn bool almostEqual(const Matrix4<Type> &m, const Matrix4<Type> &tol) const
	\brief return \f$\underline{this} ~== \underline{m}\f$ specifying the tolerance
	*/
	bool almostEqual(const Matrix4<Type> &m, const Matrix4<Type> &tol) const 
		{ return equal(m, tol); }

	/*! 
	\fn void transpose(const Matrix4<Type> &m)
	\brief Transpose matrix: \f$\underline{this} = \underline{m}^T\f$
	*/
	void transpose(const Matrix4<Type> &m) {
		m_m[0][0] = m[0][0];
		m_m[0][1] = m[1][0];
		m_m[0][2] = m[2][0];
		m_m[0][3] = m[3][0];
		m_m[1][0] = m[0][1];
		m_m[1][1] = m[1][1];
		m_m[1][2] = m[2][1];
		m_m[1][3] = m[3][1];
		m_m[2][0] = m[0][2];
		m_m[2][1] = m[1][2];
		m_m[2][2] = m[2][2];
		m_m[2][3] = m[3][2];
		m_m[3][0] = m[0][3];
		m_m[3][1] = m[1][3];
		m_m[3][2] = m[2][3];
		m_m[3][3] = m[3][3];
	}
	/*! 
	\fn void mult(const Matrix4<Type> &a, const Matrix4<Type> &b)
	\brief Matrix multiplication: \f$\underline{this} = \underline{a} * \underline{b}\f$
	*/
	void mult(const Matrix4<Type> &a, const Matrix4<Type> &b)	{
		for (int c=0; c<4; c++) {
			m_m[0][c] = a[0][0] * b[0][c] + a[0][1] * b[1][c] + a[0][2] * b[2][c] + a[0][3] * b[3][c];
			m_m[1][c] = a[1][0] * b[0][c] + a[1][1] * b[1][c] + a[1][2] * b[2][c] + a[1][3] * b[3][c];
			m_m[2][c] = a[2][0] * b[0][c] + a[2][1] * b[1][c] + a[2][2] * b[2][c] + a[2][3] * b[3][c];
			m_m[3][c] = a[3][0] * b[0][c] + a[3][1] * b[1][c] + a[3][2] * b[2][c] + a[3][3] * b[3][c];
		}
	}
	/*! 
	\fn void add(const Matrix4<Type> &a, const Matrix4<Type> &b)	
	\brief Matrix addition: \f$\underline{this} = \underline{a} + \underline{b}\f$
	*/
	void add(const Matrix4<Type> &a, const Matrix4<Type> &b) {
		m_m[0][0] = a[0][0] + b[0][0];
		m_m[0][1] = a[0][1] + b[0][1];
		m_m[0][2] = a[0][2] + b[0][2];
		m_m[0][3] = a[0][3] + b[0][3];
		m_m[1][0] = a[1][0] + b[1][0];
		m_m[1][1] = a[1][1] + b[1][1];
		m_m[1][2] = a[1][2] + b[1][2];
		m_m[1][3] = a[1][3] + b[1][3];
		m_m[2][0] = a[2][0] + b[2][0];
		m_m[2][1] = a[2][1] + b[2][1];
		m_m[2][2] = a[2][2] + b[2][2];
		m_m[2][3] = a[2][3] + b[2][3];
		m_m[3][0] = a[3][0] + b[3][0];
		m_m[3][1] = a[3][1] + b[3][1];
		m_m[3][2] = a[3][2] + b[3][2];
		m_m[3][3] = a[3][3] + b[3][3];
	}
	/*! 
	\fn void sub(const Matrix4<Type> &a, const Matrix4<Type> &b)	
	\brief Matrix subtraction: \f$\underline{this} = \underline{a} - \underline{b}\f$
	*/
	void sub(const Matrix4<Type> &a, const Matrix4<Type> &b) {
		m_m[0][0] = a[0][0] - b[0][0];
		m_m[0][1] = a[0][1] - b[0][1];
		m_m[0][2] = a[0][2] - b[0][2];
		m_m[0][3] = a[0][3] - b[0][3];
		m_m[1][0] = a[1][0] - b[1][0];
		m_m[1][1] = a[1][1] - b[1][1];
		m_m[1][2] = a[1][2] - b[1][2];
		m_m[1][3] = a[1][3] - b[1][3];
		m_m[2][0] = a[2][0] - b[2][0];
		m_m[2][1] = a[2][1] - b[2][1];
		m_m[2][2] = a[2][2] - b[2][2];
		m_m[2][3] = a[2][3] - b[2][3];
		m_m[3][0] = a[3][0] - b[3][0];
		m_m[3][1] = a[3][1] - b[3][1];
		m_m[3][2] = a[3][2] - b[3][2];
		m_m[3][3] = a[3][3] - b[3][3];
	}
	/*! 
	\fn void scale(Type s, const Matrix4<Type> &m)
	\brief Scalar scale matrix: \f$\underline{this} = s * \underline{m}\f$
	*/
	void scale(Type s, const Matrix4<Type> &m) {
		m_m[0][0] = s * m[0][0];
		m_m[0][1] = s * m[0][1];
		m_m[0][2] = s * m[0][2];
		m_m[0][3] = s * m[0][3];
		m_m[1][0] = s * m[1][0];
		m_m[1][1] = s * m[1][1];
		m_m[1][2] = s * m[1][2];
		m_m[1][3] = s * m[1][3];
		m_m[2][0] = s * m[2][0];
		m_m[2][1] = s * m[2][1];
		m_m[2][2] = s * m[2][2];
		m_m[2][3] = s * m[2][3];
		m_m[3][0] = s * m[3][0];
		m_m[3][1] = s * m[3][1];
		m_m[3][2] = s * m[3][2];
		m_m[3][3] = s * m[3][3];
	}

	/*! 
	\fn void postMult(const Matrix4<Type>& a)
	\brief Post matrix multiplication: \f$\underline{this} = \underline{this} * \underline{b}\f$
	*/
	void postMult(const Matrix4<Type>& a)
	{	Matrix4<Type> b;

		b.copy(*this);
		this->mult(b, a);
	}
		
	/*! 
	\fn void preMult(const Matrix4<Type>& b)
	\brief Pre matrix multiplication: \f$\underline{this} = \underline{b} * \underline{this}\f$
	*/
	void preMult(const Matrix4<Type>& b)
	{	mst::Matrix4<Type> a;

		a.copy(*this);
		this->mult(b, a);
	}
	/*!
	\fn Type* operator[](int i)
	\param i index
	\brief Accesses indexed row of matrix
	*/
	Type* operator[](int i) { return &m_m[i][0]; }
	/*!
	\fn const Type* operator[](int i) const
	\param i index
	\brief Accesses indexed row of matrix
	*/
	const Type* operator[](int i) const { return &m_m[i][0]; }
	/*!
	\fn bool operator==(const Matrix4<Type>& m) const
	\param m The matrix
	\brief Test for equality of a matrix with the current matrix
	*/
	bool operator==(const Matrix4<Type>& m) const { return this->equal(m); }
	/*!
	\fn bool operator!=(const Matrix4<Type>& m) const
	\param m The matrix
	\brief Test for inequality of a matrix with the current matrix
	*/
	bool operator!=(const Matrix4<Type>& m) const { return !this->equal(m); }
	/*!
	\fn void invertIdent(const Matrix4<Type>& m)
	\param m The matrix
	\brief Set the inverted identity matrix
	*/
	void invertIdent(const Matrix4<Type>& m) { this->makeIdent(); }

	/*!
	\fn bool invertFull(const Matrix4<Type>& m)
	\param m The matrix
	\brief Matrix inversion: \f$\underline{this} = \underline{m}^{-1}\f$
	*/
	bool invertFull(const Matrix4<Type>& m)
		{	int i, j, index[4];
			Type invmat[4][4], temp;

			copy(m);
			if (LUDecompMat(index)) {
				for(j = 0; j < 4; j++) {
					for(i = 0; i < 4; i++) invmat[j][i] = (Type) 0;
					invmat[j][j] = ((Type) 1);
					LUBackSubMat(index, invmat[j]);
				}
				/* transpose invmat */
				for(j = 0; j < 4; j++) {
					for(i = 0; i < j; i++) {
						temp = invmat[i][j];
						invmat[i][j] = invmat[j][i];
						invmat[j][i] = temp;
					}
				}
				set((Type*) invmat);
				return true;
			} 
			else {
				makeIdent();
				return false;
			}
		}
	/*!
	\fn void invertAff(const Matrix4<Type>& m)
	\param m The matrix
	\brief Matrix inversion: \f$\underline{this} = \underline{m}^{-1}\f$
	 affine inverse, dst = inverse(m)
	 
	 | A 0 |  has      | inv(A)    0 |
	 | C 1 |  inverse  | -C*inv(A) 1 |
	*/
	void invertAff(const Matrix4<Type>& m)
		{	Type det, rdet;
			int i, j;

			/*
			* do 3X3 Invert by adjoint method 
			*/
			det = MST_DET3(m[0][0], m[0][1], m[0][2],
						m[1][0], m[1][1], m[1][2],
						m[2][0], m[2][1], m[2][2]);
			if (mst::abslt(det, mst::epsilon(det))) {
				mstWarn("No inverse: singular matrix\n");
				det = (Type) 1;
			}
			rdet = ((Type) 1) / det;
			m_m[0][0] =  rdet * MST_DET2(m[1][1], m[1][2], m[2][1], m[2][2]);
			m_m[1][0] = -rdet * MST_DET2(m[1][0], m[1][2], m[2][0], m[2][2]);
			m_m[2][0] =  rdet * MST_DET2(m[1][0], m[1][1], m[2][0], m[2][1]);
			m_m[0][1] = -rdet * MST_DET2(m[0][1], m[0][2], m[2][1], m[2][2]);
			m_m[1][1] =  rdet * MST_DET2(m[0][0], m[0][2], m[2][0], m[2][2]);
			m_m[2][1] = -rdet * MST_DET2(m[0][0], m[0][1], m[2][0], m[2][1]);
			m_m[0][2] =  rdet * MST_DET2(m[0][1], m[0][2], m[1][1], m[1][2]);
			m_m[1][2] = -rdet * MST_DET2(m[0][0], m[0][2], m[1][0], m[1][2]);
			m_m[2][2] =  rdet * MST_DET2(m[0][0], m[0][1], m[1][0], m[1][1]);

			for (i = 0; i < 3; i++) {	/* compute -C*inv(A) */
				m_m[3][i] = (Type) 0;
				for (j = 0; j < 3; j++) m_m[3][i] -= m[3][j] * m_m[j][i];
			}
			m_m[0][3] = m_m[1][3] = m_m[2][3] = (Type) 0;
			m_m[3][3] = (Type) 1;
		}

	/*!
	\fn void invertOrtho(const Matrix4<Type>& m)
	\brief Invert the ortogonal matrix.
	*/
	void invertOrtho(const Matrix4<Type>& m)
		{	Type det, rdet = (Type) 1;
			int i, j;

			det = MST_DET3(m[0][0], m[0][1], m[0][2], /* upper left 3x3   */
					m[1][0], m[1][1], m[1][2],
					m[2][0], m[2][1], m[2][2]);
			if (mst::abslt(det, mst::epsilon(det))) {
				mstWarn("singular matrix");
				det = (Type) 1;
			}
			rdet = mst::pow(det, Type(-0.666666666));
			for (i = 0; i < 3; i++) /* scale 3x3 by transpose/det */
				for (j = 0; j < 3; j++) m_m[i][j] = rdet * m[j][i];

			m_m[3][3] = (Type) 1;
			for (i = 0; i < 3; i++) { /* compute -C*inv(A) */
				m_m[3][i] = (Type) 0;
				for (j = 0; j < 3; j++) m_m[3][i] -= m[3][j] * m_m[j][i];
			}
			m_m[0][3] = m_m[1][3] = m_m[2][3] = (Type) 0;
			m_m[3][3] = (Type)1;
		}
	/*!
	\fn void invertOrthoN(const Matrix4<Type>& m)
	\brief orthonormal inverse

	orthonormal inverse, dst = inverse(m)
	 
	  | A 0 |  has      | inv(A)    0 |
	  | C 1 |  inverse  | -C*inv(A) 1 |
	 
	  Orthonormal is easier because inv(A) = transpose(A)
	*/
	void invertOrthoN(const Matrix4<Type>& m)
		{
			this->transpose(m);
			for (int i=0; i<3; i++) {
				m_m[3][i] = (Type) 0;
				// compute -C*inv(A)
				for (int j = 0; j < 3; j++)
					m_m[3][i] -= m[3][j] * m_m[j][i];
			}
			m_m[0][3] = m_m[1][3] = m_m[2][3] = (Type) 0;
			m_m[3][3] = (Type) 1;
		}
	/*! 
	\fn void preTrans(Type x, Type y, Type z, const Matrix4<Type>& m)
	\brief pre translate matrix: \f$\underline{this} = \underline{T}(x,y,z)*\underline{m}\f$
	*/
	void preTrans(Type x, Type y, Type z, const Matrix4<Type>& m)
		{	mst::Matrix4<Type> s, a;

			a.copy(m);
			s.makeTrans(x, y, z);
			mult(s, a);
		}
	/*! 
	\fn void postTrans(const Matrix4<Type>& m, Type x, Type y, Type z)
	\brief post translate matrix: \f$\underline{this} = \underline{m}*\underline{T}(x,y,z)\f$
	*/
	void postTrans(const Matrix4<Type>& m, Type x, Type y, Type z)
		{	mst::Matrix4<Type> s,a;

			a.copy(m);
			s.makeTrans(x, y, z);
			mult(a, s);
		}

	/*! 
	\fn void preEuler(Type roll, Type pitch, Type yaw, const Matrix4<Type>& m)
	\brief pre euler matrix: \f$\underline{this} = \underline{E}(roll,pitch,yaw)*\underline{m}\f$
	*/
	void preEuler(Type roll, Type pitch, Type yaw, const Matrix4<Type>& m)
		{	mst::Matrix4<Type> s,a;

			a.copy(m);
			s.makeEuler(roll, pitch, yaw);
			mult(s, a);
		}

	/*! 
	\fn void postEuler(const Matrix4<Type>& m, Type roll, Type pitch, Type yaw)
	\brief post euler matrix: \f$\underline{this} = \underline{m}*\underline{E}(roll,pitch,yaw)\f$
	*/
	void postEuler(const Matrix4<Type>& m, Type roll, Type pitch, Type yaw)
		{	mst::Matrix4<Type> s, a;

			a.copy(m);
			s.makeEuler(roll, pitch, yaw);
			mult(a, s);
		}
	/*! 
	\fn void preRot(Type radians, Type x, Type y, Type z, const Matrix4<Type>& m)
	\brief pre rotate matrix: \f$\underline{this} = \underline{R}(radians,x,y,z)*\underline{m}\f$
	*/
	void preRot(Type radians, Type x, Type y, Type z, const Matrix4<Type>& m)
		{	mst::Matrix4<Type> s, a;

			a.copy(m);
			s.makeRot(radians, x, y, z);
			mult(s, a);
		}

	/*! 
	\fn void postRot(const Matrix4<Type>& m, Type radians, Type x, Type y, Type z)
	\brief post rotate matrix: \f$\underline{this} = \underline{m}*\underline{R}(radians,x,y,z)\f$
	*/
	void postRot(const Matrix4<Type>& m, Type radians, Type x, Type y, Type z)
		{	mst::Matrix4<Type> s, a;

			a.copy(m);
			s.makeRot(radians, x, y, z);
			mult(a, s);
		}
	/*! 
	\fn void preScale(Type x, Type y, Type z, const Matrix4<Type>& m)
	\brief pre scale matrix: \f$\underline{this} = \underline{S}(x,y,z)*\underline{m}\f$
	*/
	void preScale(Type x, Type y, Type z, const Matrix4<Type>& m)
		{	 mst::Matrix4<Type> s, a;

			a.copy(m);
			s.makeScale(x, y, z);
			mult(s, a);
		}
			
	/*! 
	\fn void postScale(const Matrix4<Type>& m, Type x, Type y, Type z)
	\brief post scale matrix: \f$\underline{this} = \underline{m}*\underline{S}(x,y,z)\f$
	*/
	void postScale(const Matrix4<Type>& m, Type x, Type y, Type z)
		{	mst::Matrix4<Type> s, a;

			a.copy(m);
			s.makeScale(x, y, z);
			mult(a, s);
		}

public:	// Warning: return by value can be quite slow
	/*! 
	\fn Matrix4<Type> operator*(const Matrix4<Type>& m) const
	\brief multiply two matrices
	*/
	Matrix4<Type> operator*(const Matrix4<Type>& m) const {
		Matrix4<Type> dst; dst.mult(*this, m); return dst;
	}
	/*! 
	\fn Matrix4<Type> operator+(const Matrix4<Type>& m) const
	\brief add two matrices
	*/
	Matrix4<Type> operator+(const Matrix4<Type>& m) const {
		Matrix4<Type> dst; dst.add(*this, m); return dst;
	}

	/*! 
	\fn Matrix4<Type> operator-(const Matrix4<Type>& m) const
	\brief subtract two matrices
	*/
	Matrix4<Type> operator-(const Matrix4<Type>& m) const {
		Matrix4<Type> dst; dst.sub(*this, m); return dst;
	}

	/*! 
	\fn friend Matrix4<Type> operator*(Type s, const Matrix4<Type>& m)
	\brief pre multiply a matrix by a scalar
	*/
	friend Matrix4<Type> operator*(Type s, const Matrix4<Type>& m)
		{ Matrix4<Type> dst; dst.scale(s, m); return dst; }
		
	/*! 
	\fn friend Matrix4<Type> operator*(const Matrix4<Type>& m, Type s)
	\brief post multiply a matrix by a scalar
	*/
	friend Matrix4<Type> operator*(const Matrix4<Type>& m, Type s)
		{ Matrix4<Type> dst; dst.scale(s, m); return dst; }
	
	/*! 
	\fn friend Matrix4<Type> operator/(const Matrix4<Type>& m, Type s)
	\brief divide a matrix by a scalar
	*/
	friend Matrix4<Type> operator/(const Matrix4<Type>& m, Type s)
		{ Matrix4<Type> dst; dst.scale(((Type) 1)/s, m); return dst; }

	/*! 
	\fn Matrix4<Type>& operator=(const Matrix4<Type>& m)
	\brief Assignment operator: \f$\underline{this} = \underline{m}\f$
	 */
	Matrix4<Type>& operator=(const Matrix4<Type>& m) {
		m_m[0][0] = m[0][0];
		m_m[0][1] = m[0][1];
		m_m[0][2] = m[0][2];
		m_m[0][3] = m[0][3];
		m_m[1][0] = m[1][0];
		m_m[1][1] = m[1][1];
		m_m[1][2] = m[1][2];
		m_m[1][3] = m[1][3];
		m_m[2][0] = m[2][0];
		m_m[2][1] = m[2][1];
		m_m[2][2] = m[2][2];
		m_m[2][3] = m[2][3];
		m_m[3][0] = m[3][0];
		m_m[3][1] = m[3][1];
		m_m[3][2] = m[3][2];
		m_m[3][3] = m[3][3];
		return *this;
	}

	/*! 
	\fn Matrix4<Type>& operator*=(const Matrix4<Type>& m)
	\brief Post multiply operator: \f$\underline{this} = \underline{this}*\underline{m}\f$
	*/
	Matrix4<Type>& operator*=(const Matrix4<Type>& m)  {
		this->postMult(m); return *this;
	}
	/*! 
	\fn Matrix4<Type>& operator*=(Type s)
	\brief Scalar multiply operator: \f$\underline{this} = s * \underline{this}\f$
	*/
	Matrix4<Type>& operator*=(Type s) {
		m_m[0][0] *= s;
		m_m[0][1] *= s;
		m_m[0][2] *= s;
		m_m[0][3] *= s;
		m_m[1][0] *= s;
		m_m[1][1] *= s;
		m_m[1][2] *= s;
		m_m[1][3] *= s;
		m_m[2][0] *= s;
		m_m[2][1] *= s;
		m_m[2][2] *= s;
		m_m[2][3] *= s;
		m_m[3][0] *= s;
		m_m[3][1] *= s;
		m_m[3][2] *= s;
		m_m[3][3] *= s;
		return *this;
	}
	/*! 
	\fn  Matrix4<Type>& operator/=(Type s)
	\brief Scalar divide operator: \f$\underline{this} = 1/s * \underline{this}\f$
	 */
	Matrix4<Type>& operator/=(Type s) { *this *= (((Type) 1)/s); return *this; }

	/*! 
	\fn  Matrix4<Type>& operator+=(const Matrix4<Type>& m)
	\brief Addition operator: \f$\underline{this} = \underline{this}+\underline{m}\f$
	 */
	Matrix4<Type>& operator+=(const Matrix4<Type>& m) {
		m_m[0][0] += m[0][0];
		m_m[0][1] += m[0][1];
		m_m[0][2] += m[0][2];
		m_m[0][3] += m[0][3];
		m_m[1][0] += m[1][0];
		m_m[1][1] += m[1][1];
		m_m[1][2] += m[1][2];
		m_m[1][3] += m[1][3];
		m_m[2][0] += m[2][0];
		m_m[2][1] += m[2][1];
		m_m[2][2] += m[2][2];
		m_m[2][3] += m[2][3];
		m_m[3][0] += m[3][0];
		m_m[3][1] += m[3][1];
		m_m[3][2] += m[3][2];
		m_m[3][3] += m[3][3];
		return *this;
	}
	/*! 
	\fn  Matrix4<Type>& operator-=(const Matrix4<Type>& m)
	\brief Subtraction operator: \f$\underline{this} = \underline{this}-\underline{m}\f$
	 */
	Matrix4<Type>& operator-=(const Matrix4<Type>& m) {
		m_m[0][0] -= m[0][0];
		m_m[0][1] -= m[0][1];
		m_m[0][2] -= m[0][2];
		m_m[0][3] -= m[0][3];
		m_m[1][0] -= m[1][0];
		m_m[1][1] -= m[1][1];
		m_m[1][2] -= m[1][2];
		m_m[1][3] -= m[1][3];
		m_m[2][0] -= m[2][0];
		m_m[2][1] -= m[2][1];
		m_m[2][2] -= m[2][2];
		m_m[2][3] -= m[2][3];
		m_m[3][0] -= m[3][0];
		m_m[3][1] -= m[3][1];
		m_m[3][2] -= m[3][2];
		m_m[3][3] -= m[3][3];
		return *this;
	}
	/*! 
	\fn void makeIdent()
	\brief make an identity matrix
	*/
	void makeIdent() {
		m_m[0][0] = (Type) 1;
		m_m[0][1] = (Type) 0;
		m_m[0][2] = (Type) 0;
		m_m[0][3] = (Type) 0;
		m_m[1][0] = (Type) 0;
		m_m[1][1] = (Type) 1;
		m_m[1][2] = (Type) 0;
		m_m[1][3] = (Type) 0;
		m_m[2][0] = (Type) 0;
		m_m[2][1] = (Type) 0;
		m_m[2][2] = (Type) 1;
		m_m[2][3] = (Type) 0;
		m_m[3][0] = (Type) 0;
		m_m[3][1] = (Type) 0;
		m_m[3][2] = (Type) 0;
		m_m[3][3] = (Type) 1;
	}
	/*! 
	\fn void makeEuler(Type roll, Type pitch, Type heading)
	\brief make an Euler rotation matrix
	*/
	void makeEuler(Type roll, Type pitch, Type heading)
	{	Type sin_psi = mst::sin(heading);
		Type cos_psi = mst::cos(heading);
		Type sin_phi = mst::sin(roll);
		Type cos_phi = mst::cos(roll);
		Type sin_theta = mst::sin(pitch);
		Type cos_theta = mst::cos(pitch);

		m_m[0][0] = cos_theta * cos_psi;
		m_m[0][1] = cos_theta * sin_psi;
		m_m[0][2] = -sin_theta;
		m_m[0][3] = (Type) 0;

		m_m[1][0] = cos_psi * sin_phi * sin_theta - sin_psi * cos_phi;
		m_m[1][1] = sin_phi * sin_theta * sin_psi + cos_phi * cos_psi;
		m_m[1][2] = sin_phi * cos_theta;
		m_m[1][3] = (Type) 0;

		m_m[2][0] = cos_psi * cos_phi * sin_theta + sin_phi * sin_psi;
		m_m[2][1] = cos_phi * sin_psi * sin_theta - sin_phi * cos_psi;
		m_m[2][2] = cos_phi * cos_theta;
		m_m[2][3] = (Type) 0;

		m_m[3][0] = (Type) 0;
		m_m[3][1] = (Type) 0;
		m_m[3][2] = (Type) 0;
		m_m[3][3] = (Type) 1;
	}
	/*! 
	\fn void makeRot(Type radians, Type x, Type y, Type z)
	\brief make an axis rotation matrix
	*/
	void makeRot(Type radians, Type x, Type y, Type z)
	{	Type s, c, t, f;

		f = ((Type) 1) / mst::sqrt(x*x + y*y + z*z);
		x *= f;
		y *= f;
		z *= f;

		s = mst::sin(radians);
		c = mst::cos(radians);

		t = ((Type) 1) - c;

		m_m[0][0] = t * x * x + c;
		m_m[0][1] = t * x * y + s * z;
		m_m[0][2] = t * x * z - s * y;
		m_m[0][3] = (Type) 0;
		m_m[1][0] = t * y * x - s * z;
		m_m[1][1] = t * y * y + c;
		m_m[1][2] = t * y * z + s * x;
		m_m[1][3] = (Type) 0;
		m_m[2][0] = t * z * x + s * y;
		m_m[2][1] = t * z * y - s * x;
		m_m[2][2] = t * z * z + c;
		m_m[2][3] = (Type) 0;
		m_m[3][0] = (Type) 0;
		m_m[3][1] = (Type) 0;
		m_m[3][2] = (Type) 0;
		m_m[3][3] = (Type) 1;
	}
	/*! 
	\fn void makeTrans(Type x, Type y, Type z)
	\brief make a translation matrix
	*/
	void makeTrans(Type x, Type y, Type z) {
		m_m[0][0] = (Type) 1;
		m_m[0][1] = (Type) 0;
		m_m[0][2] = (Type) 0;
		m_m[0][3] = (Type) 0;
		m_m[1][0] = (Type) 0;
		m_m[1][1] = (Type) 1;
		m_m[1][2] = (Type) 0;
		m_m[1][3] = (Type) 0;
		m_m[2][0] = (Type) 0;
		m_m[2][1] = (Type) 0;
		m_m[2][2] = (Type) 1;
		m_m[2][3] = (Type) 0;
		m_m[3][0] = x;
		m_m[3][1] = y;
		m_m[3][2] = z;
		m_m[3][3] = (Type) 1;
	}
	/*! 
	\fn void makeScale(Type x, Type y, Type z)
	\brief make a scaling matrix
	*/
	void makeScale(Type x, Type y, Type z) {
		m_m[0][0] = x;
		m_m[0][1] = (Type) 0;
		m_m[0][2] = (Type) 0;
		m_m[0][3] = (Type) 0;
		m_m[1][0] = (Type) 0;
		m_m[1][1] = y;
		m_m[1][2] = (Type) 0;
		m_m[1][3] = (Type) 0;
		m_m[2][0] = (Type) 0;
		m_m[2][1] = (Type) 0;
		m_m[2][2] = z;
		m_m[2][3] = (Type) 0;
		m_m[3][0] = (Type) 0;
		m_m[3][1] = (Type) 0;
		m_m[3][2] = (Type) 0;
		m_m[3][3] = (Type) 1;
	}

protected:
	/*! 
	\fn bool LUDecompMat(int index[4])
	\brief perform an LU decomposition of a matrix

	
	 Perform an LU decomposition of a matrix.  
	 From Numerical Recipes in C, pg 43
	 Destroys the original matrix.  The resulting matrix is as follows
	
	      a b c d         1 0 0 0      a b c d
	 LU = e f g h     L = e 1 0 0  U = 0 f g h
	      i j k l	        i j 1 0      0 0 k l
	      m n o p	        m n o 1      0 0 0 p
	*/
	bool LUDecompMat(int index[4])
		{	int i, imax, j, k;
			Type big, dum, sum, temp;
			Type vv[4];

			for(i = 0; i < 4; i++) {
				big = (Type) 0;
				for(j = 0; j < 4; j++) 
					if((temp = mst::abs(m_m[i][j])) > big) big = temp;
				if(mst::abslt(big, mst::epsilon(big))) {
					m_m[i][i] = mst::epsilon(big);
					big = m_m[i][i];
					mstWarn("Singular matrix in routine LUDCMP");
					return false;
				}
				vv[i] = ((Type) 1)/ big;
		    }

			for(j = 0; j < 4; j++) {
				for(i = 0; i < j; i++) {
					sum = m_m[i][j];
					for(k = 0; k < i; k++) 
						sum -= m_m[i][k] * m_m[k][j];
					m_m[i][j] = sum;
				}

				big = (Type) 0;
				for(i = j; i < 4; i++) {
					sum = m_m[i][j];
					for(k = 0; k < j; k++) 
						sum -= m_m[i][k] * m_m[k][j];
					m_m[i][j] = sum;
					if((dum = vv[i] * mst::abs(sum)) >= big) {
						big = dum;
						imax = i;
					}
				}
				if(j != imax) {
					for(k = 0; k < 4; k++) {
						dum = m_m[imax][k];
						m_m[imax][k] = m_m[j][k];
						m_m[j][k] = dum;
					}
					vv[imax] = vv[j];
				}
				index[j] = imax;

				if(m_m[j][j] == 0.0f) m_m[j][j] = 1.0e-20f;
				if(j != 4 - 1) {
					dum = ((Type) 1)/ (m_m[j][j]);
					for(i = j + 1; i < 4; i++) m_m[i][j] *= dum;
				}
			}
			return true;
		}
	/*! 
	\fn void LUBackSubMat(int index[4], Type b[4])
	\brief LU back substitute matrix
	*/
	void LUBackSubMat(int index[4], Type b[4])
		{	int i, ii = -1, ip, j;
			Type sum;

			for(i = 0; i < 4; i++) {
				ip = index[i];
				sum = b[ip];
				b[ip] = b[i];
				if(ii >= 0)
					for(j = ii; j <= i - 1; j++)
						sum -= m_m[i][j] * b[j];
				else if(sum) ii = i;
				b[i] = sum;
			}

			for(i = 4 - 1; i >= 0; i--) {
				sum = b[i];
				for(j = i + 1; j < 4; j++) sum -= m_m[i][j]*b[j];
				b[i] = sum / m_m[i][i];
			}
		}

private:
	Type m_m[4][4];
};

/*! 
\typedef Matrix4f
\brief float specialisation of a 4x4 Matrix
\ingroup math
*/
typedef Matrix4<float> Matrix4f;

/*! 
\typedef Matrix4d
\brief double specialisation of a 4x4 Matrix
\ingroup math
*/
typedef Matrix4<double> Matrix4d;

/*! 
\fn template<class Type> std::ostream& operator<<(std::ostream &s, const mst::Matrix4<Type> &v)
\brief stream output operator of a Matrix4
\ingroup io
*/
template<class Type> std::ostream& operator<<(std::ostream &s, const mst::Matrix4<Type> &m)
	{ 
		s << m[0][0] << " " << m[0][1] << " " << m[0][2] << " " << m[0][3] << std::endl
		  << m[1][0] << " " << m[1][1] << " " << m[1][2] << " " << m[1][3] << std::endl
		  << m[2][0] << " " << m[2][1] << " " << m[2][2] << " " << m[2][3] << std::endl
		  << m[3][0] << " " << m[3][1] << " " << m[3][2] << " " << m[3][3];
		return s; 
	}

/*! 
\fn template<class Type> std::istream& operator>>(std::istream &s, mst::Matrix4<Type> &v)
\brief stream input operator of a Matrix4
\ingroup io
*/
template<class Type> std::istream& operator>>(std::istream &s, mst::Matrix4<Type> &m)
	{ 	char c;
		s >> m[0][0] >> c >> m[0][1] >> c >> m[0][2] >> c >> m[0][3]
		  >> m[1][0] >> c >> m[1][1] >> c >> m[1][2] >> c >> m[1][3]
		  >> m[2][0] >> c >> m[2][1] >> c >> m[2][2] >> c >> m[2][3]
		  >> m[3][0] >> c >> m[3][1] >> c >> m[3][2] >> c >> m[3][3];
		return s; 
	}

}

#endif

